/**
 * @file color.c
 * @author sevenfite (lin481399413@163.com)
 * @brief 颜色识别
 * @version 0.1
 * @date 2024-07-06
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include <stdbool.h>
#include "color.h"
#include "i2c.h"
#include "C_code_Log.h"
#include "myconfig.h"

//0:H 1:S 2:L 3:R 4:G 5:B
static uint8_t read[6];

#ifdef CAMERA_COLOR
#include "usart_port.h"
#include "usart.h"
#include "camera.h"
USART_Data usartColor;
uint8_t USART_buf3[50];
Color colorFlag;
static int camera_getcolor(char *data, int len);
#endif
/**
 * @brief iic复位
 * @note 发送9个CLK上升沿，然后发送一个STOP信号，解除从机的SDA占用
 * 
 */
static void I2C_Reset()
{
  HAL_I2C_MspDeInit(&hi2c1);
  hi2c1.State = HAL_I2C_STATE_RESET;
  MX_I2C1_Init();
  // 硬件i2c会出现死锁，当超时次数达到一定数量，即很有可能是发生了死锁
  // 所谓死锁是指主机与从机互相等待，主机以为总线在从机手上控制，从机以为总线在主机手上控制，一直再等待对方释放总线
}
/**
 * @brief 颜色传感器初始化
 * 
 */
void colorInit(){
#ifdef CAMERA_COLOR
    USART_DataTypeInit(&usartColor, &huart2, USART_buf3, sizeof(USART_buf3), DMA_MODE, NULL);
    LOG_INFO("Color init\n");
#else
    uint8_t temp;
    HAL_I2C_Mem_Read(&hi2c1,color_ReadADDRESS,PING_REG,I2C_MEMADD_SIZE_8BIT,&temp,1,0xFF);
    if(temp == 0x66)
    {
      HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
      LOG_INFO("颜色传感器初始化成功");
    }
#endif
}
bool colorRead();
/**
 * @brief 颜色识别
 * 
 * @return Color 红绿黄
 */
Color colorRecognize(){
#ifdef CAMERA_COLOR
  while(1){
    if (USART_DataIsReceived(&usartCamera))
    {
      USART_DataResetReceivedFlag(&usartColor);
      int dataSize = 0;
      dataSize = USART_DataGetReceivedLen(&usartColor);

      USART_GetData(&usartColor)[dataSize] = '\0';  // 把最后一位改成\0
      LOG_INFO("%s",(char*)USART_GetData(&usartColor));

      int data = camera_getcolor((char*)USART_GetData(&usartColor), dataSize);
      if (data >= 0)
      {
        if(data == 0)
          colorFlag = COLOR_UNKNOW;
        else if(data == 1)
          colorFlag = COLOR_GREEN;
        else if(data == 2)
          colorFlag = COLOR_YELLOW;
        else if(data == 3)
          colorFlag = COLOR_RED;
      }
      break;
    }
  }
  return colorFlag;
#else
  colorRead();
  if(read[0] > 230 && read[0] < 250){
    return COLOR_RED;
  }
  else if(read[0] > 20 && read[0] < 40){
    return COLOR_YELLOW;
  }
  else if(read[0] > 70 && read[0] < 120){
    return COLOR_GREEN;
  }
  else{
    return COLOR_UNKNOW;
  }
#endif
}

#ifdef CAMERA_COLOR
static int camera_getcolor(char *data, int len) {
  int result;
  result = json_parsedInt(data,"color");
  return result;
}
#else

/**
 * @brief 读取颜色传感器数据
 * 
 * @return true 表示接收成功
 * @return false 表示接收失败
 */
bool colorRead(){
    int busy = 0;
    HAL_StatusTypeDef status;
    while (busy < 10) {
      status = HAL_I2C_Mem_Read(&hi2c1, color_ReadADDRESS, Read_HSL_REG, I2C_MEMADD_SIZE_8BIT, read,
                                3, 0xFF);
      if (status == HAL_BUSY) {
        busy++;
        if (busy == 10) {
          LOG_ERROR("iic忙碌");
          return false;
        }
        HAL_Delay(1);
      } else if (status == HAL_ERROR) {        
        LOG_ERROR("颜色传感器读取失败");
        HAL_Delay(1);
        I2C_Reset();
        HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
        return false;
      } else {
        busy = 0;
        break;
      }
    }
    // while (busy < 10) {
    //   status = HAL_I2C_Mem_Read(&hi2c1, color_ReadADDRESS, Read_RGB_REG, I2C_MEMADD_SIZE_8BIT, &read[3],
    //                             3, 0xFF);
    //   if (status == HAL_BUSY) {
    //     busy++;
    //     if (busy == 10) {
    //       LOG_ERROR("iic忙碌");
    //       return false;
    //     }
    //     HAL_Delay(1);
    //   } else if (status == HAL_ERROR) {
    //     LOG_ERROR("颜色传感器读取失败");
    //     HAL_Delay(1);
    //     I2C_Reset();
    //     HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
    //     return false;
    //   } else {
    //     busy = 0;
    //     break;
    //   }
    // }
    return true;
}
#endif


/*----------------------Test----------------------------*/
#include "display.h"
#include "ABS_key.h"
#include "myconfig.h"
#include "servo.h"
My_StatusTypeDef ColorTest(int key_Num) {
#ifdef CAMERA_COLOR
  colorRecognize();
  displayNum(colorFlag,Display_firstCol,Display_firstRow);
#else
  //舵机旋转到颜色传感器位置
  setServoAngleByIndex(SERVO_COLOR,SERVO_COLOR_SCAN);
  colorRead();
  displayStr("H:", Display_firstCol, Display_firstRow,RED);
  displayNum(read[0],Display_firstCol+Display_OneChar*2,Display_firstRow);
  displayStr("S:", Display_firstCol, Display_secondRow,RED);
  displayNum(read[1],Display_firstCol+Display_OneChar*2,Display_secondRow);
  displayStr("L:", Display_firstCol, Display_thirdRow,RED);
  displayNum(read[2],Display_firstCol+Display_OneChar*2,Display_thirdRow);
#ifdef USE_LCD  // 使用LCD显示屏
  // 将888格式的颜色转换为565格式
  uint16_t r = (read[3] >> 3) & 0x1F;  // 8位红色转换为5位
  uint16_t g = (read[4] >> 2) & 0x3F;  // 8位绿色转换为6位
  uint16_t b = (read[5] >> 3) & 0x1F;  // 8位蓝色转换为5位
                                       // 将颜色分量组合成16位颜色
  uint16_t color565 = (r << 11) | (g << 5) | b;
  // 显示颜色
  displayColor(Display_firstCol, Display_fourthRow, color565);
#endif
#endif
  if (key_Num == Key_ESC) {
    //舵机旋转到初始位置
    setServoAngleByIndex(SERVO_COLOR,SERVO_COLOR_INIT);
    return MY_OK;
  }
  return MY_BUSY;
}